/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : freertos.c
  * Description        : Code for freertos applications
  ******************************************************************************
  * @attention
  *
  * <h2><center>&copy; Copyright (c) 2021 STMicroelectronics.
  * All rights reserved.</center></h2>
  *
  * This software component is licensed by ST under Ultimate Liberty license
  * SLA0044, the "License"; You may not use this file except in compliance with
  * the License. You may obtain a copy of the License at:
  *                             www.st.com/SLA0044
  *
  ******************************************************************************
  */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */

#include "mb.h"
#include "mbport.h"
#include "Foc.h"
#include "ParamMng.h"
#include "SEGGER_RTT.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
typedef StaticTask_t osStaticThreadDef_t;
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */

/* USER CODE END Variables */
/* Definitions for defaultTask */
osThreadId_t defaultTaskHandle;
uint32_t defaultTaskBuffer[ 128 ];
osStaticThreadDef_t defaultTaskControlBlock;
const osThreadAttr_t defaultTask_attributes = {
  .name = "defaultTask",
  .cb_mem = &defaultTaskControlBlock,
  .cb_size = sizeof(defaultTaskControlBlock),
  .stack_mem = &defaultTaskBuffer[0],
  .stack_size = sizeof(defaultTaskBuffer),
  .priority = (osPriority_t) osPriorityNormal,
};

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */

/* USER CODE END FunctionPrototypes */

void StartDefaultTask(void *argument);

void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */

/**
  * @brief  FreeRTOS initialization
  * @param  None
  * @retval None
  */
void MX_FREERTOS_Init(void) {
  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* USER CODE BEGIN RTOS_MUTEX */
  /* add mutexes, ... */
  /* USER CODE END RTOS_MUTEX */

  /* USER CODE BEGIN RTOS_SEMAPHORES */
  /* add semaphores, ... */
  /* USER CODE END RTOS_SEMAPHORES */

  /* USER CODE BEGIN RTOS_TIMERS */
  /* start timers, add new ones, ... */
  /* USER CODE END RTOS_TIMERS */

  /* USER CODE BEGIN RTOS_QUEUES */
  /* add queues, ... */
  /* USER CODE END RTOS_QUEUES */

  /* Create the thread(s) */
  /* creation of defaultTask */
  defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes);

  /* USER CODE BEGIN RTOS_THREADS */
  /* add threads, ... */
  /* USER CODE END RTOS_THREADS */

  /* USER CODE BEGIN RTOS_EVENTS */
  /* add events, ... */
  /* USER CODE END RTOS_EVENTS */

}

/* USER CODE BEGIN Header_StartDefaultTask */
/**
  * @brief  Function implementing the defaultTask thread.
  * @param  argument: Not used
  * @retval None
  */
/* USER CODE END Header_StartDefaultTask */
void StartDefaultTask(void *argument)
{
  /* USER CODE BEGIN StartDefaultTask */
    extern uint16_t usRegHoldBuf[];
    eMBErrorCode    eStatus;
    uint8_t rttCount = 0;

    eStatus = eMBInit( MB_RTU, 0x0A, 0, 115200, MB_PAR_NONE );

    /* Enable the Modbus Protocol Stack. */
    eStatus = eMBEnable(  );
  /* Infinite loop */
  for(;;)
  {
    ( void )eMBPoll(  );
    if(++rttCount == 10)
    {
      rttCount = 0;
      SEGGER_RTT_printf(0,"1:%d\r\n", focDataSt.IqPid.ref);
      SEGGER_RTT_printf(0,"2:%d\r\n", focDataSt.IqPid.fbk);
      SEGGER_RTT_printf(0,"3:%d\r\n", focDataSt.SpdPid.ref);
      SEGGER_RTT_printf(0,"4:%d\r\n", focDataSt.SpdPid.fbk);
      SEGGER_RTT_printf(0,"5:%d\r\n", focDataSt.PosPid.ref);
      SEGGER_RTT_printf(0,"6:%d\r\n", focDataSt.PosPid.fbk);
    }

    //pid参数设置
    if(usRegHoldBuf[512+0])
    {
      //停止foc计算
      focDataSt.focCalibrating = 1;

      //电流环PID
      focDataSt.IqPid.param.Kp = pParamListSt->focIqPidKp;
      focDataSt.IqPid.param.Ki = pParamListSt->focIqPidKi;
      focDataSt.IqPid.param.Kd = pParamListSt->focIqPidKd;
      focDataSt.IdPid.param.Kp = pParamListSt->focIdPidKp;
      focDataSt.IdPid.param.Ki = pParamListSt->focIdPidKi;
      focDataSt.IdPid.param.Kd = pParamListSt->focIdPidKd;
      arm_pid_init_q15(&focDataSt.IqPid.param, 0);
      arm_pid_init_q15(&focDataSt.IdPid.param, 0);

      //速度环PID
      focDataSt.SpdPid.param.Kp = pParamListSt->focSpdPidKp;
      focDataSt.SpdPid.param.Ki = pParamListSt->focSpdPidKi;
      focDataSt.SpdPid.param.Kd = pParamListSt->focSpdPidKd;
      arm_pid_init_q15(&focDataSt.SpdPid.param, 0);

      //位置环PID
      focDataSt.PosPid.param.Kp = pParamListSt->focPosPidKp;
      focDataSt.PosPid.param.Ki = pParamListSt->focPosPidKi;
      focDataSt.PosPid.param.Kd = pParamListSt->focPosPidKd;
      arm_pid_init_q15(&focDataSt.PosPid.param, 0);

      //参数保存
      paramMngWrite();
      usRegHoldBuf[512+0] = 0;
      focDataSt.focCalibrating = 0;
    }

    //校准
    if(usRegHoldBuf[512+1])
    {
      focCalibrate();
      usRegHoldBuf[512+1] = 0;
    }

    osDelay(1);
  }
  /* USER CODE END StartDefaultTask */
}

/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */

/* USER CODE END Application */

/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
